import robomaster.chassis as offical
from robomaster.chassis import *

import platfrom.platform as platform
import threading,time,math

from robomaster import module
from robomaster import logger
from robomaster import protocol
from robomaster import util
from robomaster import action

from ..location.location_init import ActionMonitor
'''
class Chassis(module.Module):
'''

def _md_auto_stop_timer(self, api="drive_speed"):
    if api == "drive_speed":
        logger.info("Chassis: drive_speed timeout, auto stop!")
        self._dij_drive_speed(0, 0, 0)
    elif api == "drive_wheels":
        logger.info("Chassis: drive_wheels timeout, auto stop!")
        self.drive_wheels(0, 0, 0)
    else:
        logger.warning("Chassis: unsupported api:{0}".format(api))

    platform.DRIVE_SPEED_ADJUSTER.proto_clear()
    platform.PHYSICAL_MSG.security_monitor.event_set_by("END")

def md_drive_speed(self, x=0.0, y=0.0, z=0.0, timeout=None):
    """ 修改后的drive_speed
    """
    proto = protocol.ProtoChassisSpeedMode()
    proto._x_spd = util.CHASSIS_SPD_X_CHECKER.val2proto(x)
    proto._y_spd = util.CHASSIS_SPD_Y_CHECKER.val2proto(y)
    proto._z_spd = util.CHASSIS_SPD_Z_CHECKER.val2proto(z)

    platform.DRIVE_SPEED_ADJUSTER.proto_clear()
    platform.DRIVE_SPEED_ADJUSTER.register_proto(proto)
    platform.PHYSICAL_MSG.send_action_info(platform.encoder_proto(proto))
    
    logger.info("x_spd:{0:f}, y_spd:{1:f}, z_spd:{2:f}".format(proto._x_spd, proto._y_spd, proto._z_spd))
    if timeout:
        if not hasattr(self,'_md_auto_timer'): self._md_auto_timer = None
        if self._md_auto_timer:
            if self._md_auto_timer.is_alive():
                self._md_auto_timer.cancel()
        self._md_auto_timer = platform.simulate_msg.PlatformTimer(
            timeout, self._auto_stop_timer, args=("drive_speed",))
        self._md_auto_timer.start()
        return self._send_sync_proto(proto)

    return self._send_sync_proto(proto)

def md_move(self, x=0, y=0, z=0, xy_speed=0.5, z_speed=30):
    """ 修改后的move
    """
    action = SafeChassisMoveAction(self, x, y, z, xy_speed, z_speed)
    return action

'''
class ChassisMoveAction(action.Action):
'''
@property
def _encode_action_str(self):
    # move,x,y,z,xy_spd,z_spd
    msg = "move,{:.3f},{:.3f},{:.3f},{:.3f},{:.3f}".format(
        self._x, self._y, self._z, self._spd_xy, self._spd_z)
    return msg


'''
class SafeChassisMoveAction(ChassisMoveAction)

新加的类: SafeChassisMoveAction
用于将一个Action分阶段完成，且在阶段间插入调整动作
'''
class SafeChassisMoveAction(action.Action):
    _action_proto_cls = 1 # just not None
    _push_proto_cls = 1 # just not None

    _actual_action = None
    _chassis = None

    # _pause_event = None

    # _monitor_thread = None
    # _monitor_enable = False
    _checker_args = None
    _checker_lock = threading.Lock()
    _on_state_changed = None

    def __init__(self, chassis:Chassis , x=0, y=0, z=0, spd_xy=0, spd_z=0, **kw):
        # self._action_id = -1
        self.timer = None
        self._percent = 0

        # self._adjust_lock = threading.Lock()
        # self._adjust_time = 0

        self._x = x
        self._y = y
        self._z = z
        self._zero_element = []
        for _ in (x,y,z):
            self._zero_element.append(0 if _==0 else 1)


        self._cur_x = 0
        self._cur_y = 0
        self._cur_z = 0

        self._spd_xy = spd_xy
        self._spd_z = spd_z
        self._chassis = chassis

        self._state = action.ACTION_IDLE

        self.run_thread = threading.Thread(target=self.run_action)
        self.run_thread.start()
        
        self._event = threading.Event()
        self._left_actual_exec_time = None

    def __repr__(self):
        return "action_id:{0}, state:{1}, percent:{2}, x:{3}, y:{4}, z:{5}, xy_speed:{6}, z_speed:{7}".format(
            self._actual_action._action_id, self._state, self._percent, self._x, self._y, self._z, self._spd_xy, self._spd_z)

    def _generate_action(self):
        left = self._left()[:]
        return ChassisMoveAction(left[0],left[1],left[2],self._spd_xy,self._spd_z)

    def _goal(self):
        return [self._x,self._y,self._z]

    def _current(self):
        return [self._cur_x,self._cur_y,self._cur_z]
    
    def _left(self):
        left_x = (self._x - self._cur_x)*self._zero_element[0]
        left_y = (self._y - self._cur_y)*self._zero_element[1]
        left_z = (self._z - self._cur_z)*self._zero_element[2]

        return [left_x,left_y,left_z]

    def _update_current(self,old_pos,new_pos):
        # print("old_pos = {:.3f} {:.3f} deg = {:.3f}".format(old_pos[0],old_pos[1],old_pos[2]))
        # print("new_pos = {:.3f} {:.3f} deg = {:.3f}".format(new_pos[0],new_pos[1],new_pos[2]))

        
        det_x = new_pos[0]-old_pos[0]
        det_y = new_pos[1]-old_pos[1]
        det_deg = new_pos[2]-old_pos[2]

        rad = math.radians(old_pos[2])
        dx,dy = ActionMonitor._after_rot_matrix(-rad,det_x,det_y)

        # motion = (dx*0.001,dy*0.001,det_deg)

        self._cur_x += dx*0.001
        self._cur_y += -dy*0.001
        self._cur_z += det_deg
        print(self.percent_str)
        return 

    def wait_for_completed(self, timeout=None):
        """ 等待任务动作直到完成

        :param timeout: 超时，在timeout前未完成任务动作，直接返回
        :return: bool: 动作在指定时间内完成，返回True; 动作超时返回False
        """
        if self._event.isSet() and self.is_completed:
            return True

        if timeout:
            self.timer = threading.Timer(timeout,
                self._changeto_state, args=(action.ACTION_EXCEPTION,))
            self.timer.start()

            self._event.wait()
            # 由于adjust模块的存在，timeout计时不准确
   
            if (self.state==action.ACTION_EXCEPTION):
                print("Action: wait_for_completed timeout.")
                logger.warning("Action: wait_for_completed timeout.")
                return False
        else:
            self._event.wait()
            if not self._event.isSet():
                logger.warning("Action: wait_for_completed timeout.")
                self._changeto_state(action.ACTION_EXCEPTION)
                return False
        return True

    @property
    def percent_str(self):
        s = "cur_percent: x=({:.3f}/{:.3f}),y=({:.3f}/{:.3f}),deg=({:.3f}/{:.3f})".format(
            self._cur_x,self._x,
            self._cur_y,self._y,
            self._cur_z,self._z)
        return s

    def _start_security_monitor(self):
        print("security_monitor init")
        platform.PHYSICAL_MSG.security_monitor.start(
            api='move',check_distance=200
        )


        # platform.PHYSICAL_MSG._security_monitor_event.clear()
        # platform.PHYSICAL_MSG._security_set_msg = 'None'

        # platform.PHYSICAL_MSG.start_security_monitor(200)

    def _start_action_completed_monitor(self):
        monitor_thread = threading.Thread(target=self.action_compelted_monitor)
        monitor_thread.start()
        # print("action_completed_monitor start")

    def action_compelted_monitor(self):
        print("action_completed_monitor wait")
        timeout = None
        if (self.timer): 
            timeout = self.timer.get_left()
            print("** left execute time ={:.3f}s **".format(timeout))

        exec_result = self._actual_action.wait_for_completed(timeout)

        print("action completed")

        if (not platform.PHYSICAL_MSG.security_monitor.isSet()):
            platform.PHYSICAL_MSG.security_monitor.event_set_by('END')

        # if (not platform.PHYSICAL_MSG._security_monitor_event.isSet()):
        #     platform.PHYSICAL_MSG._security_set_msg = "END"
        #     print("monitor set by END")
        #     platform.PHYSICAL_MSG._security_monitor_event.set()

        if (not exec_result):
            platform.flush_undefined_behavior()


    def position(self,is_snapshot=False):
        tid = None
        if (is_snapshot):
            tid = platform.PHYSICAL_MSG.security_monitor.get_set_tid()

        pos = platform.PHYSICAL_MSG.query_position(tid)
        return pos

    def run_action(self):
        while(True):
            self._cur_position_security_check()
            self._once_turn()
            if (self.is_completed): break
        
        print("total action {:.3f} {:.3f} {:.3f}".format(
            self._cur_x,self._cur_y,self._cur_z
        ))

    def _cur_position_security_check(self):
        dis = platform.PHYSICAL_MSG.get_sensor_data_info()[:]
        min_id = 0
        for i in range(4):
            if dis[i]<dis[min_id]: min_id=i

        if (dis[min_id]>210): return 
        
        platform.PHYSICAL_MSG.send_adjust_status(is_on=True)
        print("start at an danger place")
        self.move_adjust(is_manual=False)
    
    def _once_turn(self):
        print("--- start _once_turn")
        self._actual_action = self._generate_action()
        print("> start ",self._actual_action._encode_action_str)

        old_pos = self.position()
        # print("old_pos = {:.3f} {:.3f} deg = {:.3f}".format(old_pos[0],old_pos[1],old_pos[2]))

        # send action
        self._start_security_monitor()
        self._start_action_completed_monitor()

        if (self._state == action.ACTION_IDLE):
            self._changeto_state(action.ACTION_RUNNING)

        self._chassis._action_dispatcher.send_action(self._actual_action)
        # print("send actual action")

        # one of monitor will set
        platform.PHYSICAL_MSG.security_monitor.wait()

        # 获取从start pos 到当前位置的移动距离
        set_reason = platform.PHYSICAL_MSG.security_monitor.get_set_msg()

        print("set_reason =",set_reason)

        new_pos = self.position(is_snapshot=(set_reason=='ADJUST'))

        # print("new_pos = {:.3f} {:.3f} deg = {:.3f}".format(new_pos[0],new_pos[1],new_pos[2]))
        # print("monitor_thread exit by",set_reason)

        self._update_current(old_pos,new_pos)

        if (set_reason == "ADJUST"):
            # set by _auto_distance_security_check()
            # _actual_action stop but not set completed,
            # action_compelted_monitor() still wait! 
            self._actual_action._changeto_state(action.ACTION_SUCCEEDED)
            print("set actual_action completed")
            self.move_adjust()

        elif (set_reason == "END"):
            # set by action_compelted_monitor()
            # _auto_distance_security_check() has been stop by set()
            print("set total action completed")
            self._changeto_state(action.ACTION_SUCCEEDED)

        print("--- end _once_turn")
        return    

    def move_adjust(self,is_manual=False):
        platform.PHYSICAL_MSG.security_monitor.adjust(api='move',obj=self,is_manual=is_manual)

    # def adjust(self,is_manual=False):
    #     self._adjust_lock.acquire()
        
    #     adjust_start_time = time.time()

    #     print("adjust start")
    #     # flush
    #     platform.flush_undefined_behavior()

    #     print(self.percent_str)
    
    #     # new_pos = self.position()
    #     # print("before adjust = {:.3f} {:.3f} deg = {:.3f}".format(new_pos[0],new_pos[1],new_pos[2]))
        
    #     if is_manual:
    #         while True:
    #             ss = input("adjust_action>")
    #             if (ss=='Q' or ss=='q'):
    #                 break
    #             for ch in ss:
    #                 platform.do_action(ch,self._chassis)
    #     else:
    #         # step1  
    #         match_action = "SAWD"
    #         dis_dir = "FRBL"
    #         while(1):
    #             dis = platform.PHYSICAL_MSG.get_sensor_data_info()[:]
    #             min_id = 0
    #             for i in range(4):
    #                 if dis[i]<dis[min_id]: min_id=i

    #             if (dis[min_id]>300): break
    #             match_distance = 300 - dis[min_id]
    #             match_distance = max(100,match_distance)

    #             # print(">>> adjust {} {} {}".format(dis_dir[min_id],
    #             # dis[min_id],match_action[min_id]))
    #             platform.do_action(match_action[min_id],self._chassis,
    #             max(0.1,match_distance*0.001))

    #         # step2 
    #         platform.do_action('B',self._chassis)

    #     # stop adjust status
    #     platform.PHYSICAL_MSG.send_adjust_status(is_on=False)
        
    #     adjust_end_time = time.time()
    #     self._adjust_time += (adjust_end_time-adjust_start_time)
    #     self._adjust_lock.release()        
    #     platform.EP_ROBOT.led.set_led(comp="all", r=0, g=200, b=0)


# modify list
offical.Chassis._dij_drive_speed = offical.Chassis.drive_speed
offical.Chassis._dij_move = offical.Chassis.move

offical.Chassis._auto_stop_timer = _md_auto_stop_timer
offical.Chassis.drive_speed = md_drive_speed
offical.Chassis.move = md_move

offical.ChassisMoveAction._encode_action_str = _encode_action_str

